/* Pendulum1 a pendulum model using the Euler-Richardson Algorithm by Ryan Haynes */

import java.text.*;
import java.awt.Color;

class Pendulum2{


	
	DecimalFormat myDec = new DecimalFormat("0.000");
	//initialize instance variables
	double damp = 0.5;
	double driveAmp = .5;
	double driveFreq = 2.0/3.0;
	double dt = 0.01;
	
	Pendulum2(){
		swing();  //convert degrees to radians when swing is called
	}
	
	public void swing() {

		//Initialize new plot
		Plot swingPlot = new Plot("Theta vs Time",0,100,1,-6*Math.PI,6*Math.PI,Math.PI/4);
		Plot logPlot = new Plot("Log vs Time",0,100,1,-35,35,Math.PI/4);

		//Initialize variables
		double alpha1;
		double alpha2;
		double omega1 = 0;
		double omega2 = 0;
		double theta1 = 0;
		double theta2 = 0.001;		
		double alpha1Mid;
		double alpha2Mid;
		double theta1Mid;
		double theta2Mid;
		double omega1Mid;
		double omega2Mid;

		
		//Start Euler Richardson algorithm
		for (double t=0;t<=100;t+=dt) {
				
			alpha1 = torque(theta1,omega1,t);
			alpha2 = torque(theta2,omega2,t);
			
			theta1Mid = theta1 + omega1 * 0.5 * dt;
			theta2Mid = theta2 + omega2 * 0.5 * dt;
			
			omega1Mid = omega1 + alpha1 * 0.5 * dt;
			omega2Mid = omega2 + alpha2 * 0.5 * dt;
			
			alpha1Mid = torque(theta1Mid,omega1Mid,t + dt/2);
			alpha2Mid = torque(theta2Mid,omega2Mid,t + dt/2);
						
			theta1 += omega1Mid * dt;
			theta2 += omega2Mid * dt;
			
			omega1 += alpha1Mid * dt;
			omega2 += alpha2Mid * dt;


			swingPlot.setColor(Color.red);
			swingPlot.addPoint(t,theta1);
			swingPlot.setColor(Color.blue);
			swingPlot.addPoint(t,theta2);
			// Make plot of log(theta2 - theta1) versus time
			double log = Math.log10(Math.abs(theta2-theta1));
			logPlot.addPoint(t,log);
		}

	}
	
	//function to calculate acceleration
	double torque(double theta, double omega, double t) {
		return -Math.sin(theta) -damp*omega + driveAmp*Math.sin(driveFreq*t);
	}
	
	public static void main(String[] args) {
		new Pendulum2();

	}
}
